using System;
using System.Runtime.CompilerServices;
using UnityEngine;

namespace Net
{
    [Serializable]
    public struct Matrix3x4 : IEquatable<Matrix3x4>
    {
        public Vector3 column0; // 第0列 (right方向)
        public Vector3 column1; // 第1列 (up方向)
        public Vector3 column2; // 第2列 (forward方向)
        public Vector3 column3; // 第3列 (position)

        // 静态实例
        private static readonly Matrix3x4 zeroMatrix = new Matrix3x4(
            Vector3.zero, Vector3.zero, Vector3.zero, Vector3.zero);

        private static readonly Matrix3x4 identityMatrix = new Matrix3x4(
            new Vector3(1, 0, 0), 
            new Vector3(0, 1, 0), 
            new Vector3(0, 0, 1), 
            Vector3.zero);

        public static Matrix3x4 zero => zeroMatrix;
        public static Matrix3x4 identity => identityMatrix;

        public bool isIdentity => 
            column0 == new Vector3(1, 0, 0) && 
            column1 == new Vector3(0, 1, 0) && 
            column2 == new Vector3(0, 0, 1) && 
            column3 == Vector3.zero;

        // 构造函数
        public Matrix3x4(Vector3 c0, Vector3 c1, Vector3 c2, Vector3 c3)
        {
            column0 = c0;
            column1 = c1;
            column2 = c2;
            column3 = c3;
        }

        // 访问器
        public float this[int row, int col]
        {
            get
            {
                return col switch
                {
                    0 => column0[row],
                    1 => column1[row],
                    2 => column2[row],
                    3 => column3[row],
                    _ => throw new IndexOutOfRangeException("Invalid column index!")
                };
            }
            set
            {
                switch (col)
                {
                    case 0: column0[row] = value; break;
                    case 1: column1[row] = value; break;
                    case 2: column2[row] = value; break;
                    case 3: column3[row] = value; break;
                    default: throw new IndexOutOfRangeException("Invalid column index!");
                }
            }
        }

        // 获取列
        public Vector3 GetColumn(int index)
        {
            return index switch
            {
                0 => column0,
                1 => column1,
                2 => column2,
                3 => column3,
                _ => throw new IndexOutOfRangeException("Invalid column index!")
            };
        }

        // 获取行 (返回Vector4，因为一行有4个元素)
        public Vector4 GetRow(int row)
        {
            return row switch
            {
                0 => new Vector4(column0.x, column1.x, column2.x, column3.x),
                1 => new Vector4(column0.y, column1.y, column2.y, column3.y),
                2 => new Vector4(column0.z, column1.z, column2.z, column3.z),
                _ => throw new IndexOutOfRangeException("Invalid row index!")
            };
        }

        // 设置列
        public void SetColumn(int index, Vector3 column)
        {
            switch (index)
            {
                case 0: column0 = column; break;
                case 1: column1 = column; break;
                case 2: column2 = column; break;
                case 3: column3 = column; break;
                default: throw new IndexOutOfRangeException("Invalid column index!");
            }
        }

        // 设置行
        public void SetRow(int row, Vector4 rowData)
        {
            switch (row)
            {
                case 0:
                    column0.x = rowData.x;
                    column1.x = rowData.y;
                    column2.x = rowData.z;
                    column3.x = rowData.w;
                    break;
                case 1:
                    column0.y = rowData.x;
                    column1.y = rowData.y;
                    column2.y = rowData.z;
                    column3.y = rowData.w;
                    break;
                case 2:
                    column0.z = rowData.x;
                    column1.z = rowData.y;
                    column2.z = rowData.z;
                    column3.z = rowData.w;
                    break;
                default:
                    throw new IndexOutOfRangeException("Invalid row index!");
            }
        }

        // 方向属性
        public Vector3 right
        {
            get => column0;
            set => column0 = value;
        }

        public Vector3 up
        {
            get => column1;
            set => column1 = value;
        }

        public Vector3 forward
        {
            get => column2;
            set => column2 = value;
        }

        public Vector3 position
        {
            get => column3;
            set => column3 = value;
        }

        public Vector3 left => -column0;
        public Vector3 down => -column1;
        public Vector3 back => -column2;

        // 旋转属性
        public Quaternion rotation
        {
            get
            {
                // 从旋转矩阵提取四元数
                Vector3 scale = new Vector3(
                    column0.magnitude,
                    column1.magnitude,
                    column2.magnitude
                );

                // 归一化旋转矩阵
                Vector3 normalizedColumn0 = column0 / scale.x;
                Vector3 normalizedColumn1 = column1 / scale.y;
                Vector3 normalizedColumn2 = column2 / scale.z;

                // 计算四元数
                float trace = normalizedColumn0.x + normalizedColumn1.y + normalizedColumn2.z;
                
                if (trace > 0)
                {
                    float s = Mathf.Sqrt(trace + 1.0f) * 2;
                    float invS = 1.0f / s;
                    return new Quaternion(
                        (normalizedColumn1.z - normalizedColumn2.y) * invS,
                        (normalizedColumn2.x - normalizedColumn0.z) * invS,
                        (normalizedColumn0.y - normalizedColumn1.x) * invS,
                        s * 0.25f
                    );
                }
                else if (normalizedColumn0.x > normalizedColumn1.y && normalizedColumn0.x > normalizedColumn2.z)
                {
                    float s = Mathf.Sqrt(1.0f + normalizedColumn0.x - normalizedColumn1.y - normalizedColumn2.z) * 2;
                    float invS = 1.0f / s;
                    return new Quaternion(
                        s * 0.25f,
                        (normalizedColumn0.y + normalizedColumn1.x) * invS,
                        (normalizedColumn2.x + normalizedColumn0.z) * invS,
                        (normalizedColumn1.z - normalizedColumn2.y) * invS
                    );
                }
                else if (normalizedColumn1.y > normalizedColumn2.z)
                {
                    float s = Mathf.Sqrt(1.0f + normalizedColumn1.y - normalizedColumn0.x - normalizedColumn2.z) * 2;
                    float invS = 1.0f / s;
                    return new Quaternion(
                        (normalizedColumn0.y + normalizedColumn1.x) * invS,
                        s * 0.25f,
                        (normalizedColumn1.z + normalizedColumn2.y) * invS,
                        (normalizedColumn2.x - normalizedColumn0.z) * invS
                    );
                }
                else
                {
                    float s = Mathf.Sqrt(1.0f + normalizedColumn2.z - normalizedColumn0.x - normalizedColumn1.y) * 2;
                    float invS = 1.0f / s;
                    return new Quaternion(
                        (normalizedColumn2.x + normalizedColumn0.z) * invS,
                        (normalizedColumn1.z + normalizedColumn2.y) * invS,
                        s * 0.25f,
                        (normalizedColumn0.y - normalizedColumn1.x) * invS
                    );
                }
            }
            set
            {
                // 设置旋转部分，保持缩放和位置不变
                Vector3 currentScale = new Vector3(
                    column0.magnitude,
                    column1.magnitude,
                    column2.magnitude
                );
                
                Matrix3x4 rotationMatrix = Rotate(value);
                column0 = rotationMatrix.column0 * currentScale.x;
                column1 = rotationMatrix.column1 * currentScale.y;
                column2 = rotationMatrix.column2 * currentScale.z;
            }
        }

        // 变换方法
        public Vector3 MultiplyPoint(Vector3 point)
        {
            return new Vector3(
                column0.x * point.x + column1.x * point.y + column2.x * point.z + column3.x,
                column0.y * point.x + column1.y * point.y + column2.y * point.z + column3.y,
                column0.z * point.x + column1.z * point.y + column2.z * point.z + column3.z
            );
        }

        public Vector3 MultiplyVector(Vector3 vector)
        {
            return new Vector3(
                column0.x * vector.x + column1.x * vector.y + column2.x * vector.z,
                column0.y * vector.x + column1.y * vector.y + column2.y * vector.z,
                column0.z * vector.x + column1.z * vector.y + column2.z * vector.z
            );
        }

        // 静态构造方法
        public static Matrix3x4 TRS(Vector3 pos, Quaternion q, Vector3 s)
        {
            Matrix3x4 m = identity;
            
            // 旋转部分
            float x = q.x * 2f;
            float y = q.y * 2f;
            float z = q.z * 2f;
            float xx = q.x * x;
            float yy = q.y * y;
            float zz = q.z * z;
            float xy = q.x * y;
            float xz = q.x * z;
            float yz = q.y * z;
            float wx = q.w * x;
            float wy = q.w * y;
            float wz = q.w * z;

            m.column0 = new Vector3(
                (1f - (yy + zz)) * s.x,
                (xy + wz) * s.x,
                (xz - wy) * s.x
            );

            m.column1 = new Vector3(
                (xy - wz) * s.y,
                (1f - (xx + zz)) * s.y,
                (yz + wx) * s.y
            );

            m.column2 = new Vector3(
                (xz + wy) * s.z,
                (yz - wx) * s.z,
                (1f - (xx + yy)) * s.z
            );

            m.column3 = pos;
            return m;
        }

        public static Matrix3x4 Translate(Vector3 translation)
        {
            Matrix3x4 m = identity;
            m.column3 = translation;
            return m;
        }

        public static Matrix3x4 Rotate(Quaternion rotation)
        {
            return TRS(Vector3.zero, rotation, Vector3.one);
        }

        public static Matrix3x4 Scale(Vector3 scale)
        {
            Matrix3x4 m = identity;
            m.column0 = new Vector3(scale.x, 0, 0);
            m.column1 = new Vector3(0, scale.y, 0);
            m.column2 = new Vector3(0, 0, scale.z);
            return m;
        }

        // 矩阵乘法
        public static Matrix3x4 operator *(Matrix3x4 lhs, Matrix3x4 rhs)
        {
            Matrix3x4 res;
            
            // 计算旋转部分
            res.column0 = new Vector3(
                lhs.column0.x * rhs.column0.x + lhs.column1.x * rhs.column0.y + lhs.column2.x * rhs.column0.z,
                lhs.column0.y * rhs.column0.x + lhs.column1.y * rhs.column0.y + lhs.column2.y * rhs.column0.z,
                lhs.column0.z * rhs.column0.x + lhs.column1.z * rhs.column0.y + lhs.column2.z * rhs.column0.z
            );

            res.column1 = new Vector3(
                lhs.column0.x * rhs.column1.x + lhs.column1.x * rhs.column1.y + lhs.column2.x * rhs.column1.z,
                lhs.column0.y * rhs.column1.x + lhs.column1.y * rhs.column1.y + lhs.column2.y * rhs.column1.z,
                lhs.column0.z * rhs.column1.x + lhs.column1.z * rhs.column1.y + lhs.column2.z * rhs.column1.z
            );

            res.column2 = new Vector3(
                lhs.column0.x * rhs.column2.x + lhs.column1.x * rhs.column2.y + lhs.column2.x * rhs.column2.z,
                lhs.column0.y * rhs.column2.x + lhs.column1.y * rhs.column2.y + lhs.column2.y * rhs.column2.z,
                lhs.column0.z * rhs.column2.x + lhs.column1.z * rhs.column2.y + lhs.column2.z * rhs.column2.z
            );

            // 计算平移部分
            res.column3 = new Vector3(
                lhs.column0.x * rhs.column3.x + lhs.column1.x * rhs.column3.y + lhs.column2.x * rhs.column3.z + lhs.column3.x,
                lhs.column0.y * rhs.column3.x + lhs.column1.y * rhs.column3.y + lhs.column2.y * rhs.column3.z + lhs.column3.y,
                lhs.column0.z * rhs.column3.x + lhs.column1.z * rhs.column3.y + lhs.column2.z * rhs.column3.z + lhs.column3.z
            );

            return res;
        }

        // 与Vector3的乘法
        public static Vector3 operator *(Matrix3x4 lhs, Vector3 rhs)
        {
            return lhs.MultiplyPoint(rhs);
        }

        // 比较运算符
        public static bool operator ==(Matrix3x4 lhs, Matrix3x4 rhs)
        {
            return lhs.column0 == rhs.column0 && 
                   lhs.column1 == rhs.column1 && 
                   lhs.column2 == rhs.column2 && 
                   lhs.column3 == rhs.column3;
        }

        public static bool operator !=(Matrix3x4 lhs, Matrix3x4 rhs)
        {
            return !(lhs == rhs);
        }

        // 与Unity Matrix4x4的转换
        public static implicit operator UnityEngine.Matrix4x4(Matrix3x4 m)
        {
            return new UnityEngine.Matrix4x4(
                new Vector4(m.column0.x, m.column0.y, m.column0.z, 0),
                new Vector4(m.column1.x, m.column1.y, m.column1.z, 0),
                new Vector4(m.column2.x, m.column2.y, m.column2.z, 0),
                new Vector4(m.column3.x, m.column3.y, m.column3.z, 1)
            );
        }

        public static implicit operator Matrix3x4(UnityEngine.Matrix4x4 m)
        {
            return new Matrix3x4(
                new Vector3(m.m00, m.m10, m.m20),
                new Vector3(m.m01, m.m11, m.m21),
                new Vector3(m.m02, m.m12, m.m22),
                new Vector3(m.m03, m.m13, m.m23)
            );
        }

        // 其他方法
        public override bool Equals(object obj) => obj is Matrix3x4 other && Equals(other);
        public bool Equals(Matrix3x4 other) => this == other;
        
        public override int GetHashCode()
        {
            return column0.GetHashCode() ^ column1.GetHashCode() << 2 ^ 
                   column2.GetHashCode() >> 2 ^ column3.GetHashCode() >> 1;
        }

        public override string ToString()
        {
            return $"{column0.x:F5}\t{column1.x:F5}\t{column2.x:F5}\t{column3.x:F5}\n" +
                   $"{column0.y:F5}\t{column1.y:F5}\t{column2.y:F5}\t{column3.y:F5}\n" +
                   $"{column0.z:F5}\t{column1.z:F5}\t{column2.z:F5}\t{column3.z:F5}";
        }
    }
}